#include <iostream>
#include "d_Math.h"

using namespace d_Math;

d_Mat4f::d_Mat4f()
{
	for(int i=0; i<4; i++) 
		for(int j=0; j<4; j++)
			p[i][j] = 0;
}

d_Mat4f::d_Mat4f(float a00, float a01, float a02, float a03,
	           float a10, float a11, float a12, float a13,
	           float a20, float a21, float a22, float a23,
	           float a30, float a31, float a32, float a33)
{
    p[0][0]= a00; p[0][1] = a01; p[0][2] = a02; p[0][3] = a03;
    p[1][0]= a10; p[1][1] = a11; p[1][2] = a12; p[1][3] = a13;
    p[2][0]= a20; p[2][1] = a21; p[2][2] = a22; p[2][3] = a23;
    p[3][0]= a30; p[3][1] = a31; p[3][2] = a32; p[3][3] = a33;
}      
	     

void d_Mat4f::diagonal(float x)
{
	for(int i=0;i<4;i++)
		p[i][i] = x;
}

void d_Mat4f::print()
{
	std::cout << "[";
	for (int i=0; i<3; i++) 
	{
		std::cout << "[" << p[i][0] << "," << p[i][1] << "," << p[i][2] << "," << p[i][3] << "]" << std::endl;
	}
	std::cout << "[" << p[3][0] << "," << p[3][1] << "," << p[3][2] << "," << p[3][3] << "]]";
}

float &d_Mat4f::operator () (int ir, int ic)
{ 
	if(ir>4 || ic>4)
	{
		std::cout << "Bad index for matrix!" << std::endl;
		return p[0][0];
	}
	return p[ir][ic]; 
}

float d_Mat4f::operator () (int ir, int ic) const
{ 
	if(ir>4 || ic>4)
	{
		std::cout << "Bad index for matrix!" << std::endl;
		return p[0][0];
	}
	return p[ir][ic]; 
}

d_Vec4f d_Mat4f::operator * (d_Vec4f v)
{
	d_Vec4f result;
	for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
			result[i] += p[i][j]*v[j];
	return result;
}

d_Mat4f d_Mat4f::operator * (d_Mat4f m)
{
	d_Mat4f res;
	double sum;

	for (int i=0; i<4; i++)
		for (int j=0; j<4; j++){
		    sum = 0;
		    for (int k=0; k<4; k++)
		     	sum+= p[i][k] * m(k,j);        
		         
		    res(i,j) += sum;
		}
	return res;
}

void d_Mat4f::operator *= (d_Mat4f m)
{
	d_Mat4f res;
	double sum;

	for (int i=0; i<4; i++)
		for (int j=0; j<4; j++){
		    sum = 0;
		    for (int k=0; k<4; k++)
		     	sum+= p[i][k] * m(k,j);        
		         
		    res(i,j) += sum;
		}
	*this = res;

}

d_Mat4f d_Mat4f::operator / (float x)
{
	d_Mat4f res=(*this);
	for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
			res(i,j)/=x;
	return res;
}



